#include "Core.h"
#include "CoreHeap.h"

unsigned char Heap[PAGE_MAX * PAGE_SIZE];
stBlock *HeapAvailable;
stBlock *HeapUsed;

stBlock * Heap_FindAvailable(HEAP_INT Pages);
stBlock * Heap_FindBlock(stBlock *pBlock, stBlock *pChain);

UInt16 Heap_Used(void)
{
    stBlock * p = HeapUsed;
    UInt16 n = 0;
    if (p == NULL) return 0;
    if (p->NextBlock == HEP_CHAIN_END) return p->PageNum;
    while (1){
        n += p->PageNum;
        if (p->NextBlock == HEP_CHAIN_END) break;
        p = (stBlock *)&Heap[p->NextBlock * PAGE_SIZE];
    }

    return n;
}

UInt16 Heap_Unused(void)
{
    stBlock * p = HeapAvailable;
    UInt16 n = 0;
    if (p == NULL) return 0;
    if (p->NextBlock == HEP_CHAIN_END) return p->PageNum;
    while (1){
        n += p->PageNum;
        if (p->NextBlock == HEP_CHAIN_END) break;
        p = (stBlock *)&Heap[p->NextBlock * PAGE_SIZE];
    }

    return n;
}


void Heap_Init()
{
    LOCK_INTERRUPT;
    HeapAvailable = (stBlock *)Heap;
    HeapAvailable->PageIndex = 0;
    HeapAvailable->PageNum = PAGE_MAX;
    HeapAvailable->NextBlock = HEP_CHAIN_END;

    HeapUsed = NULL;
    UNLOCK_INTERRUPT;
}

stBlock * Heap_FindAvailable(HEAP_INT Pages)
{
	stBlock *p;
	p = HeapAvailable;

	if (p == NULL) {
	    return p;
	}

    if (p->PageNum >= Pages) {
        return p;
    }
	while (p->NextBlock != HEP_CHAIN_END) {
        p = (stBlock *)&Heap[p->NextBlock * PAGE_SIZE];
        if (p->PageNum >= Pages) {
            return p;
        }
	}

	return NULL;
}

stBlock * Heap_FindBlock(stBlock *pBlock, stBlock *pChain)
{
	stBlock *p;
	stBlock *pNext = NULL;
	stBlock *pPre;
	p = pChain;
	pPre = pBlock;

	if (p != NULL) {

		do {
            if (pBlock->PageIndex <= p->PageIndex) {
                return pPre;
            }

            if (p->NextBlock == HEP_CHAIN_END)
            {
                return p;
            }

			pNext = (stBlock *)&Heap[p->NextBlock * PAGE_SIZE];
            pPre = p;
            p = pNext;

		} while (1);
	}
	return p;
}

stBlock * Heap_FindBestMatch(HEAP_INT Pages)
{
	stBlock *p;
	stBlock *pNext = NULL;
	stBlock *pPre;
	p = HeapAvailable;
	pPre = NULL;

	if (p != NULL) {
		do {
            if (Pages <= p->PageNum) {
                if(pPre != NULL){
                    if (p->PageNum < pPre->PageNum) {
                        pPre = p;
                    }
                } else {
                    pPre = p;
                }
            }

            if (p->NextBlock == HEP_CHAIN_END)
            {
                return pPre;
            }

			pNext = (stBlock *)&Heap[p->NextBlock * PAGE_SIZE];
            p = pNext;

		} while (1);
	}
	return pPre;
}


void * Heap_Request(UInt16 size)
{
	stBlock * pBlock;
    HEAP_INT NeedPages;
    HEAP_INT NeedMore;

    NeedPages = (size + sizeof(stBlock)) / PAGE_SIZE;
    NeedMore = (size + sizeof(stBlock)) % PAGE_SIZE;
    if (NeedMore) {
        NeedPages++;
    }

    LOCK_INTERRUPT;
    //pBlock = Heap_FindAvailable(NeedPages);
    pBlock = Heap_FindBestMatch(NeedPages);
    if (pBlock != NULL) {
    	HEAP_INT NewBlockPage;
    	stBlock * pTmp;

    	if (pBlock->PageNum == NeedPages) {
            pTmp = Heap_FindBlock(pBlock,HeapAvailable);
            if (pTmp == pBlock)
            {
                if (pTmp->NextBlock == HEP_CHAIN_END) {
                    HeapAvailable = NULL;
                } else {
                    HeapAvailable = (stBlock *)&Heap[pTmp->NextBlock * PAGE_SIZE];
                }
            } else {
                pTmp->NextBlock = pBlock->NextBlock;
            }
    	} else {
            pBlock->PageNum -= NeedPages;
            NewBlockPage = pBlock->PageIndex + pBlock->PageNum;
            pBlock = (stBlock *)&Heap[NewBlockPage * PAGE_SIZE];
            pBlock->PageIndex = NewBlockPage;
            pBlock->PageNum = NeedPages;
    	}

    	pTmp = Heap_FindBlock(pBlock,HeapUsed);
    	if (pTmp == NULL) {
            pBlock->NextBlock = HEP_CHAIN_END;
            HeapUsed = pBlock;
    	} else if (pTmp == pBlock) {
            pBlock->NextBlock = HeapUsed->PageIndex;
            HeapUsed = pBlock;
    	} else {
    		pBlock->NextBlock = pTmp->NextBlock;
    		pTmp->NextBlock = pBlock->PageIndex;
    	}

        pBlock = (stBlock *)&Heap[pBlock->PageIndex * PAGE_SIZE + sizeof(stBlock)];
    }

    UNLOCK_INTERRUPT;
    return pBlock;
}

void * Heap_Release(void *pBuf)
{
    stBlock *p;
    stBlock *pTmp;
    p = (stBlock *)((unsigned char *)pBuf - sizeof(stBlock));

    LOCK_INTERRUPT;
    pTmp = Heap_FindBlock(p,HeapUsed);

    if (pTmp != NULL) {
        if (pTmp != p){
            pTmp->NextBlock = p->NextBlock;
        } else {
            if (HeapUsed->NextBlock != HEP_CHAIN_END) {
                HeapUsed= (stBlock *)&Heap[HeapUsed->NextBlock * PAGE_SIZE];
            } else {
                HeapUsed = NULL;
            }
        }
    }

    pTmp = Heap_FindBlock(p,HeapAvailable);
    if (pTmp != NULL) {
        if (pTmp == p) {
            p->NextBlock = HeapAvailable->PageIndex;
            HeapAvailable = p;
        } else {
            p->NextBlock = pTmp->NextBlock;
            pTmp->NextBlock = p->PageIndex;
        }

        if ((p->PageIndex + p->PageNum) == p->NextBlock) {
            stBlock *pNext = (stBlock *)&Heap[p->NextBlock * PAGE_SIZE];
            p->PageNum += pNext->PageNum;
            p->NextBlock = pNext->NextBlock;
        }

        if ((pTmp->PageIndex + pTmp->PageNum) == p->PageIndex) {
            pTmp->PageNum += p->PageNum;
            pTmp->NextBlock = p->NextBlock;
        }
    } else {
        HeapAvailable = p;
        p->NextBlock = HEP_CHAIN_END;
    }

    UNLOCK_INTERRUPT;
    return NULL;
}


/*
#define TEST_NUM 100
int main(void)
{
    unsigned char *pp[TEST_NUM];
    int i,n,k;
    int Fail = 0;
    UInt16 used,unused,sum;
    Heap_Init();

    for(i=0;i<TEST_NUM;i++)
        pp[i] = NULL;

    srand((unsigned)time(NULL));

    for(i=0;i<TEST_NUM * 50;i++) {
        n = rand()%TEST_NUM;
        k = rand()%200;
        if (pp[n] == NULL) {
            pp[n] = Heap_Request(k);
            if (pp[n] == NULL) Fail++;
        } else {
            pp[n] = Heap_Release(pp[n]);
        }

        used = Heap_Used();
        unused = Heap_Unused();
        sum = used + unused;

        printf("used = %d\t unused = %d\t sum = %d\t Fail = %d \n",used,unused,sum,Fail);
    }

    for(i=0;i<TEST_NUM;i++) {
        if (pp[i] != NULL) {
            pp[i] = Heap_Release(pp[i]);
        }
    }

    return 1;
}


int main(void)
{
    unsigned char *p0 = NULL;
    unsigned char *p1 = NULL;
    unsigned char *p2 = NULL;
    unsigned char *p3 = NULL;
    unsigned char *p4 = NULL;
    unsigned char *p5 = NULL;
    unsigned char *p6 = NULL;
    unsigned char *p7 = NULL;
    unsigned char *p8 = NULL;
    unsigned char *p9 = NULL;

    Heap_Init();

    p0 = Heap_Request(15);
    p1 = Heap_Request(15);
    p2 = Heap_Request(15);
    p3 = Heap_Request(15);
    p4 = Heap_Request(15);
    p5 = Heap_Request(15);
    p6 = Heap_Request(15);

    p4 = Heap_Release(p4);
    p5 = Heap_Release(p5);

    p7 = Heap_Request(15);
    p8 = Heap_Request(15);
    p9 = Heap_Request(15);

    p6 = Heap_Release(p6);
    p7 = Heap_Release(p7);

    p4 = Heap_Request(15);
    p5 = Heap_Request(15);

    p8 = Heap_Release(p8);
    p9 = Heap_Release(p9);

    return 1;
}
*/

